import asyncio
import csv
import json
import math
import os.path
import random
import threading
import time
from datetime import datetime

import redis
from PySide2.QtCore import *
from pyproj import Proj
from shiboken2 import wrapInstance

from TESS_PythonAPI.Tessng import *
from utils.net_uilts import point_in_polygon, haversine, calculate_polygon_center

# 用户插件子类，代表用户自定义与仿真相关的实现逻辑，继承自PyCustomerSimulator
#     多重继承中的父类QObject，在此目的是要能够自定义信号signlRunInfo

car_ids = dict()
car_send_ids = dict()
car_real_type = {0: 0, 1: 1, 2: 2, 3: 3, 4: 4, 5: 5, 6: 6, 7: 4, 8: 8, 9: 1, 10: 10, 11: 11, 15: 15, 16: 16, 17: 17,
                 18: 18}
car_tess_type = {1: 1, 2: 7, 3: 3, 4: 2, 5: 6, 6: 5, 7: 4, 8: 8}
car_color = {0: f"#FFFFFF", 1: f"#FFFFFF", 2: f"#000000", 3: f"#FF0000", 4: f"#C0C0C0", 5: f"#FFFF00", 6: f"#4169E1",
             7: f"#00FF00", 8: f"#802A2A", 9: f"#C0C0C0"}
car_imitate_real_type = {1: 1, 2: 4, 3: 6, 4: 1, 5: 1, 0: 1}
car_imitate_tess_type = {1: 1, 2: 7, 3: 5, 4: 1, 5: 1}
send_data = dict()


class CustomSimulator(QObject, PyCustomerSimulator):
    signalRunInfo = Signal(str)
    forStopSimu = Signal()
    forReStartSimu = Signal()

    def __init__(self, config_dict: dict):
        QObject.__init__(self)
        PyCustomerSimulator.__init__(self)
        # 当前正在仿真计算的路网名称
        self.mNetPath = None
        # 相同路网连续仿真次数
        self.mSimuCount = 0
        self.scene_dict = config_dict['scene_dict']
        # 背景车避让距离
        self.avoidDistance = 50
        # 自然交通流
        self.natural_traffic_flow = {}
        # 自然交通流
        self.back_traffic_flow = {}
        # 主车
        self.target_start_car_dict = {}
        # 主车列表
        self.target_all_car_dict = {}
        # 主车当前所在区域
        self.target_car_index = 0
        # 批次号
        self.batchNum = 0
        # 经纬度转化公式
        self.p = Proj(config_dict['proj'])
        self.type = config_dict['type']
        self.result_target_trace = 'CA001MatchResultData'
        self.result_simulator_trace = 'CA002MatchResultData'
        # 背景交通流结束标识
        self.send_data_flag = False
        # redis相关配置项
        self.redis_client = redis.StrictRedis(host='192.168.4.10', port=32056, db=0)
        # 主车所在区域
        self.target_in_area = 6
        self.target_in_area_flag = False
        self.target_tigger_flag = False
        self.target_lon = None
        self.target_lat = None
        # 行人穿行，自行车参数
        self.people_pass = 0
        self.i = 0

        # 走走停停场景参数
        self.car_slow_fast_car = dict()
        self.car_slow_fast_batch_num = 0
        # 0-正常 1-30km/h  2-50km/h
        self.car_slow_fast_status = 0
        self.car_slow_fast_batch = 0
        # 合流区场景参数
        self.confluence_batch = 0
        # 减速场景参数
        self.car_slow_dict = dict()
        self.car_slow_batch = 0
        # 背景车超车场景
        self.car_over_target_car = dict()
        # 主车汇入
        self.target_confluence_batch = 0
        # 交叉口直行
        self.intersection_z3_right_car = dict()
        self.intersection_z3_straight_car = dict()
        # 交叉口左转
        self.intersection_k2_right_car = dict()
        self.intersection_k2_straight_car = dict()
        self.bike = dict()
        self.back_traffic_K1_start_flow = dict()
        # 读取目标车辆轨迹
        service_target_thread = threading.Thread(target=self.read_target_traffic_flow)
        service_target_thread.start()
        # server = MyServer()
        # server_thread = threading.Thread(target=server.start_websocket_server)
        # server_thread.start()

    def read_target_traffic_flow(self):
        # try:
        print("自动驾驶车辆监听已就绪")
        pubsub = self.redis_client.pubsub()
        pubsub.subscribe(self.result_target_trace)
        # 开始监听消息
        for message in pubsub.listen():
            if message['type'] == 'message':
                receive_data = message['data'].decode('utf-8')
                data = json.loads(receive_data)
                json_data = data['value']
                for real_car in json_data:
                    id = real_car['id']
                    lon = real_car['longitude']
                    lat = real_car['latitude']
                    point = lon, lat
                    if self.target_in_area != 0 and self.target_in_area < 10:
                        scene_config = self.scene_dict.get(self.target_in_area)
                        # 场景区
                        scene_area = scene_config['scene_area']
                        # 触发区
                        scene_tigger = scene_config['scene_tigger']
                        if point_in_polygon(point, scene_tigger):
                            self.target_tigger_flag = True
                        if not point_in_polygon(point, scene_area):
                            self.target_in_area_flag = False
                    if not self.target_in_area_flag:
                        if self.target_in_area == 9:
                            scene_config = self.scene_dict.get(1)
                            # print("获取1的配置项")
                        else:
                            scene_config = self.scene_dict.get(self.target_in_area + 1)
                        # 场景区
                        scene_area = scene_config['scene_area']
                        scene_tigger = scene_config['scene_tigger']
                        if point_in_polygon(point, scene_area):
                            if self.target_in_area == 9:
                                self.target_in_area = 0
                            elif self.target_in_area == 0:
                                if point_in_polygon(point, scene_tigger):
                                    self.target_tigger_flag = True
                                    self.target_in_area = 1
                                    self.target_in_area_flag = True
                            elif self.target_in_area == 6:
                                if point_in_polygon(point, scene_tigger):
                                    self.target_tigger_flag = True
                                    self.target_in_area = 7
                                    self.target_in_area_flag = True
                            else:
                                self.target_in_area = self.target_in_area + 1
                                self.target_in_area_flag = True
                                self.target_tigger_flag = False
                            print("触发场景", self.target_in_area)
                    self.target_all_car_dict[id] = real_car
        # except Exception as e:
        #     print("读取目标异常", e)

    def ref_beforeStart(self, ref_keepOn):
        iface = tessngIFace()
        # 当前路网名
        tmp_net_path = iface.netInterface().netFilePath()
        if tmp_net_path != self.mNetPath:
            self.mNetPath = tmp_net_path
            self.mSimuCount = 1
        else:
            self.mSimuCount += 1
        # 可在此设置本次仿真参数
        ref_keepOn.value = True

    # 设置本类实现的过载方法被调用频次，即多少个计算周期调用一次。过多的不必要调用会影响运行效率
    def setStepsPerCall(self, vehi):
        # 设置当前车辆及其驾驶行为过载方法被TESSNG调用频次，即多少个计算周调用一次指定方法。如果对运行效率有极高要求，可以精确控制具体车辆或车辆类型及具体场景相关参数
        iface = tessngIFace()
        netface = iface.netInterface()
        netFileName = netface.netFilePath()
        # 允许对车辆重绘方法的调用
        vehi.setIsPermitForVehicleDraw(True)
        # 计算限制车道方法每10个计算周期被调用一次
        vehi.setSteps_calcLimitedLaneNumber(1)
        # 计算安全变道距离方法每10个计算周期被调用一次
        vehi.setSteps_calcChangeLaneSafeDist(1)
        vehi.setSteps_reCalcToLeftFreely(1)
        vehi.setSteps_reCalcToRightFreely(1)
        # 重新计算车辆期望速度方法每一个计算周期被调用一次
        vehi.setSteps_reCalcdesirSpeed(1)
        # 重新设置车速方法每一个计算周期被调用一次
        vehi.setSteps_reSetSpeed(1)
        # 范例打开临时路段会会创建车辆方阵，需要进行一些仿真过程控制

        simuface = iface.simuInterface()
        # 仿真精度，即每秒计算次数
        steps = simuface.simuAccuracy()

    # 过载的父类方法， 初始化车辆，在车辆启动上路时被TESS NG调用一次
    def initVehicle(self, vehi):
        # 设置当前车辆及其驾驶行为过载方法被TESSNG调用频次，即多少个计算周调用一次指定方法。如果对运行效率有极高要求，可以精确控制具体车辆或车辆类型及具体场景相关参数
        self.setStepsPerCall(vehi)
        # 车辆ID，不含首位数，首位数与车辆来源有关，如发车点、公交线路
        tmpId = vehi.id() % 100000
        # 车辆所在路段名或连接段名
        roadName = vehi.roadName()
        # 车辆所在路段ID或连接段ID
        roadId = vehi.roadId()
        return True

    # def ref_reSetFollowingType(self, vehi, ref_outTypeValue):
    #     if vehi.id() in self.car_slow_fast_car.keys():
    #         print('id强制撤销变道', vehi.id())
    #         ref_outTypeValue.value = False
    #         return True
    #     return False

    def ref_beforeToLeftFreely(self, vehi, ref_keepOn):
        if vehi.id() in self.car_slow_fast_car.keys() or vehi.id() in self.bike.keys():
            ref_keepOn.value = False

    def ref_beforeToRightFreely(self, vehi, ref_keepOn):
        if vehi.id() in self.car_slow_fast_car.keys() or vehi.id() in self.bike.keys():
            ref_keepOn.value = False

    # 过载的父类方法重新计算加速度
    def ref_calcAcce(self, vehi, acce):
        return False

    # 过载的父类方法，重新计算期望速度
    # vehi：车辆
    # ref_esirSpeed：返回结果,ref_desirSpeed.value是TESS NG计算好的期望速度，可以在此方法改变它
    # return结果：False：TESS NG忽略此方法作的修改，True：TESS NG采用此方法所作修改
    def ref_reCalcdesirSpeed(self, vehi, ref_desirSpeed):
        if vehi.id() in self.bike.keys():
            ref_desirSpeed.value = m2p(2.7)
            return True
        if vehi.id() in self.back_traffic_K1_start_flow.keys():
            f_vehicle = vehi.vehicleFront()
            if f_vehicle is not None:
                if f_vehicle in self.target_start_car_dict.keys():
                    print('前车是主车，减速至18km/h')
                    if vehi.vehiDistFront() < 30:
                        ref_desirSpeed.value = m2p(5)
                        return True
        if vehi.id() in self.car_slow_fast_car.keys():
            if self.batchNum - self.car_slow_fast_batch_num > 50:
                self.car_slow_fast_batch_num = self.batchNum
                # 判断下当前车辆的状态
                if self.car_slow_fast_status == 0 or self.car_slow_fast_status == 2:
                    #
                    # print("更新车辆速度-快速", self.car_slow_fast_status, self.car_slow_fast_batch_num, self.batchNum,
                    #       vehi.currSpeed() * 3.6)
                    ref_desirSpeed.value = m2p(8.3)
                    self.car_slow_fast_status = 1
                    return True
                elif self.car_slow_fast_status == 1:
                    # print("更新车辆速度-慢速", self.car_slow_fast_status, self.car_slow_fast_batch_num, self.batchNum,
                    #       vehi.currSpeed() * 3.6)
                    ref_desirSpeed.value = m2p(2.7)
                    self.car_slow_fast_status = 2
                    return True
            else:
                if self.car_slow_fast_status == 1:
                    # print("实时更新-慢速", self.car_slow_fast_status, self.car_slow_fast_batch_num, self.batchNum,
                    #       vehi.currSpeed() * 3.6)
                    ref_desirSpeed.value = m2p(2.7)
                    return True
                elif self.car_slow_fast_status == 2:
                    # print("实时更新-快速", self.car_slow_fast_status, self.car_slow_fast_batch_num, self.batchNum,
                    #       vehi.currSpeed() * 3.6)
                    ref_desirSpeed.value = m2p(8.3)
                    return True
                else:
                    ref_desirSpeed.value = m2p(8.3)
                    return True
        return False

    # 过载的父类方法，重新计算跟驰参数：时距及安全距离
    # vehi:车辆
    # ref_inOutSi，安全时距，ref_inOutSi.value是TESS NG已计算好的值，此方法可以改变它
    # ref_inOutSd，安全距离，ref_inOutSd.value是TESS NG已计算好的值，此方法可以改变它
    # return结果：False：TESS NG忽略此方法作的修改，True：TESS NG采用此方法所作修改
    def ref_reSetFollowingParam(self, vehi, ref_inOutSi, ref_inOutSd):
        roadName = vehi.roadName()
        if roadName == "连接段2":
            ref_inOutSd.value = m2p(30)
            return True
        return False

    # 过载的父类方法，重新计算加速度
    # vehi：车辆
    # inOutAce：加速度，inOutAcce.value是TESS NG已计算的车辆加速度，此方法可以改变它
    # return结果：False：TESS NG忽略此方法作的修改，True：TESS NG采用此方法所作修改
    def ref_reSetAcce(self, vehi, inOutAcce):
        roadName = vehi.roadName()
        if roadName == "连接段1":
            if vehi.currSpeed() > m2p(20 / 3.6):
                inOutAcce.value = m2p(-5)
                return True
            elif vehi.currSpeed() > m2p(20 / 3.6):
                inOutAcce.value = m2p(-1)
                return True
        return False

    # 过载的父类方法，重新计算当前速度
    # vehi:车辆
    # ref_inOutSpeed，速度ref_inOutSpeed.value，是已计算好的车辆速度，此方法可以改变它
    # return结果：False：TESS NG忽略此方法作的修改，True：TESS NG采用此方法所作修改
    def ref_reSetSpeed(self, vehi, ref_inOutSpeed):

        return False

    def reCalcDismissChangeLane(self, vehi):
        if vehi.id() in self.car_slow_fast_car.keys():
            return True

    # 过载的父类方法，计算是否要左自由变道
    # vehi:车辆
    # return结果，True：变道、False：不变道
    def reCalcToLeftFreely(self, vehi):
        if vehi.id() in self.car_over_target_car.keys():
            f_vehicle = vehi.vehicleFront()
            if f_vehicle is not None:
                if f_vehicle.id() in self.target_start_car_dict.keys():
                    # 主车在前
                    if vehi.vehiDistFront() < 15 and vehi.lane().number() % 2 == 0:
                        print("前車车是自驾车，需要像左变道", vehi.vehiDistFront(), vehi.lane().number())
                        return True
            lr_vehicle = vehi.vehicleLRear()
            if lr_vehicle is not None:
                if lr_vehicle.id() in self.target_start_car_dict.keys():
                    print("左后车是自驾车，需要像左变道", vehi.vehiDistLLaneRear())
                    if vehi.vehiDistLLaneRear() > 10:
                        return True
        return False

    # 过载的父类方法，计算是否要右自由变道
    # vehi:车辆
    # return结果，True：变道、False：不变道
    def reCalcToRightFreely(self, vehi):
        if vehi.id() in self.car_over_target_car.keys():
            f_vehicle = vehi.vehicleFront()
            if f_vehicle is not None:
                if f_vehicle.id() in self.target_start_car_dict.keys():
                    # 主车在前
                    # s = p2m(vehi.vehiDistFront())

                    if vehi.vehiDistFront() < 15 and vehi.lane().number() % 2 != 0:
                        print("前車车是自驾车，需要像右变道", vehi.vehiDistFront(), vehi.lane().number())
                        return True
            rr_vehicle = vehi.vehicleRRear()
            if rr_vehicle is not None:
                if rr_vehicle.id() in self.target_start_car_dict.keys():
                    print("右后车是自驾车，需要像右变道", vehi.vehiDistRLaneRear())
                    if vehi.vehiDistRLaneRear() > 10:
                        return True
        return False

    # 过载父类方法，设置信号灯色。此例设置ID为5的信号灯色为红色，此方法在每个计算周被调用，所以这个信号灯一直是红色
    # 本次自动创建的路网没有信号灯，在此实现该方法只为说明如何运用
    def calcLampColor(self, signalLamp):
        return False

    # 过载父类方法， 计算车辆当前限制车道序号列表
    def calcLimitedLaneNumber(self, vehi):
        # 如果当前车辆在路段上，且路段ID等于2，则小车走内侧，大车走外侧
        if vehi.roadIsLink():
            # IVehicle.road()方法获取的是车辆当前所路段或连接段的void指针，需要将它转换成路段或连接段
            link = wrapInstance(vehi.road().__int__(), ILink)
            if link is not None and link.id() == 2:
                laneCount = link.laneCount()
                # 小车走内侧，大车走外侧，设长度小于8米为小车
                if (vehi.length() < m2p(8)):
                    return [num for num in range(laneCount // 2 - 1)]
                else:
                    return [num for num in range(laneCount // 2 - 1, laneCount)]
        return []

    # 过载父类方法，车道限速
    def ref_calcSpeedLimitByLane(self, link, laneNumber, ref_outSpeed):
        return False

    # 过载父类方法 对发车点一增加发车时间段
    def calcDynaDispatchParameters(self):
        # TESSNG 顶层接口
        iface = tessngIFace()
        currSimuTime = iface.simuInterface().simuTimeIntervalWithAcceMutiples()
        if currSimuTime % (10 * 1000) == 0 and currSimuTime < 60 * 1000:
            # ID 等于 5 路段上车辆
            lVehi = iface.simuInterface().vehisInLink(47)
            if currSimuTime < 1000 * 30 or len(lVehi) > 0:
                return []
            else:
                now = datetime.now()
                # 当前时间秒
                currSecs = now.hour * 3600 + now.minute * 60 + now.second
                # 仿真 10 秒后且 ID 等于 1 的路段上车辆数为 0，则为 ID 等于 1 的发车点增加发车

                di = Online.DispatchInterval()
                # 动作控制案例-机动车交叉口 L5 路段发车点 ID 为 11
                di.dispatchId = 11
                di.fromTime = currSecs
                di.toTime = di.fromTime + 300 - 1
                di.vehiCount = 300
                di.mlVehicleConsDetail = [Online.VehiComposition(1, 60), Online.VehiComposition(2, 40)]
                return [di]
        return []

    # 在该方法中，通过控制车流比例调整进入道路中的车辆
    # 过载父类方法，动态修改决策点不同路径流量比
    def calcDynaFlowRatioParameters(self):
        return []

    def calcDynaSignalContralParameters(self):
        return []

    # 过载父类方法，停止指定车辆运行，退出路网，但不会从内存删除，会参数各种统计
    #  范例车辆进入ID等于2的路段或连接段，路离终点小于100米，则驰出路网
    def isStopDriving(self, vehi):
        return False

    # 过载的父类方法，TESS NG 在每个计算周期结束后调用此方法，大量用户逻辑在此实现，注意耗时大的计算要尽可能优化，否则影响运行效率
    def afterOneStep(self):
        # try:
        # ===============================全局变量===================================================
        # 首先定义全局变量，用于在不同方法内互相通信
        global car_real_type, car_color, send_data, car_tess_type
        # ===============================车辆顶层接口===============================================
        # TESSNG 顶层接口
        iface = tessngIFace()
        # TESSNG 仿真子接口
        simu_iface = iface.simuInterface()
        # TESSNG 路网子接口
        net_iface = iface.netInterface()
        dispatch_points = net_iface.dispatchPoints()
        batch_num = simu_iface.batchNumber()
        # 当前仿真计算批次
        self.batchNum = batch_num
        # ================================车辆全域数据==============================================
        all_vehicle_start = simu_iface.allVehiStarted()
        # ==================================数据处理===============================================
        # 判断是否超时未发
        timestamp = int(time.time() * 1000)
        current_time = datetime.now().strftime('%Y-%m-%d %H:%M:%S:%f')[:-3]
        # 车辆移除操作
        objects = []
        vehicle_objects = dict()
        for vehicle in all_vehicle_start:
            c_id = vehicle.id()
            speed = vehicle.currSpeed()
            angle = vehicle.angle()
            pos = vehicle.pos()
            acc = vehicle.acce()
            lon = pos.x()
            lat = pos.y()
            v_type = vehicle.vehicleTypeCode()
            longitude, latitude = self.p(lon, -lat, inverse=True)
            # print(longitude, ',', latitude, ',', angle)
            # if c_id in self.car_slow_fast_car.keys():
            #     print('走走停停', c_id, speed * 3.6)
            if c_id in self.target_start_car_dict.keys():
                continue
            if speed < 1:
                acc = 0
            if math.isnan(acc):
                acc = 0
            vehicle_json = {
                'timestamp': current_time,
                'globalTimeStamp': timestamp,
                'frameId': batch_num,
                'id': c_id,
                'name': '从车',
                'longitude': longitude,
                'latitude': latitude,
                'courseAngle': angle,
                'vehicleType': car_tess_type.get(v_type),
                'speed': speed * 3.6,
                'acc': acc
            }
            # 移除车辆
            if self.target_in_area in self.scene_dict.keys():
                scene_config = self.scene_dict.get(self.target_in_area)
                # 场景区
                scene_area = scene_config['scene_area']
                point = longitude, latitude
                # if not point_in_polygon(point, scene_area):
                #     # print(scene_area, "移除车辆", c_id, point)
                #     simu_iface.stopVehicleDriving(vehicle)
            # elif self.target_in_area == 0:
            #     scene_config = self.scene_dict.get(6)
            #     # 场景区
            #     scene_area = scene_config['scene_area']
            #     point = longitude, latitude
            #     if not point_in_polygon(point, scene_area):
            #         # print(scene_area, "移除车辆", c_id, point)
            #         simu_iface.stopVehicleDriving(vehicle)
            vehicle_objects[c_id] = vehicle_json
            objects.append(vehicle_json)
        send_json = {
            "timestamp": timestamp,
            "timestampType": 'CREATE_TIME',
            "value": objects
        }
        simulator_trace = json.dumps(send_json)
        send_data = simulator_trace
        self.redis_client.publish(self.result_simulator_trace, simulator_trace)
        # ==============================发车逻辑处理===========================================
        if self.target_in_area in self.scene_dict.keys():
            # 场景配置参数
            scene_config = self.scene_dict.get(self.target_in_area)
            scene_id = scene_config['scene_id']
            # print(scene_id, self.target_in_area_flag, self.target_tigger_flag)
            # 判断主车是否位于测试区，同时触发标识是否被触发
            if self.target_in_area_flag and self.target_tigger_flag:
                if scene_id == 'R1':
                    # 首先把其他区域的数据置空
                    self.car_slow_fast_car = dict()
                    self.car_slow_fast_batch = 0
                    self.confluence_batch = 0
                    self.car_slow_dict = dict()
                    self.car_slow_batch = 0
                    self.car_over_target_car = dict()
                    self.target_confluence_batch = 0
                    # 交叉口直行
                    self.intersection_z3_right_car = dict()
                    self.intersection_z3_straight_car = dict()
                    # 交叉口左转
                    self.intersection_k2_right_car = dict()
                    self.intersection_k2_straight_car = dict()
                    # 自行车
                    self.bike = dict()
                    self.back_traffic_K1_start_flow = dict()
                    # 行人穿行
                    people_send_flag = False
                    if self.batchNum - self.people_pass > 80:
                        # 直接行人发车
                        dvp = Online.DynaVehiParam()
                        dvp.vehiTypeCode = 7
                        dvp.laneNumber = 0
                        dvp.roadId = 599
                        dvp.dist = 0
                        dvp.speed = 2
                        self.people_pass = self.batchNum + 60
                        dvp.color = car_color[self.target_in_area]
                        simu_iface.createGVehicle(dvp, True)
                    else:
                        if batch_num == self.people_pass:
                            # self.people_pass = self.batchNum + 150
                            # 直接行人发车
                            dvp = Online.DynaVehiParam()
                            dvp.vehiTypeCode = 7
                            dvp.laneNumber = 0
                            dvp.roadId = 599
                            dvp.dist = 0
                            dvp.speed = 2
                            self.people_pass = self.batchNum + 60
                            dvp.color = car_color[self.target_in_area]
                            simu_iface.createGVehicle(dvp, True)
                elif scene_id == 'R2':
                    # 走走停停
                    self.people_pass = 0
                    self.bike = dict()
                    self.confluence_batch = 0
                    self.car_slow_dict = dict()
                    self.car_slow_batch = 0
                    self.car_over_target_car = dict()
                    self.target_confluence_batch = 0
                    # 交叉口直行
                    self.intersection_z3_right_car = dict()
                    self.intersection_z3_straight_car = dict()
                    # 交叉口左转
                    self.intersection_k2_right_car = dict()
                    self.intersection_k2_straight_car = dict()
                    self.back_traffic_K1_start_flow = dict()
                    if len(self.car_slow_fast_car) < 1:
                        self.car_slow_fast_batch_num = self.batchNum
                        self.car_slow_fast_status = 0
                        self.car_slow_fast_batch = self.batchNum + random.randint(1, 50)
                        # 需要控制的车，发车
                        dvp = Online.DynaVehiParam()
                        dvp.vehiTypeCode = 1
                        dvp.laneNumber = 0
                        dvp.roadId = 7
                        dvp.dist = 100
                        dvp.speed = 13.8
                        dvp.color = f"#C0C0C0"
                        vehicle = simu_iface.createGVehicle(dvp, True)
                        self.car_slow_fast_car[vehicle.id()] = vehicle.id()
                    else:
                        back_car_send_flag = False
                        if self.batchNum - self.car_slow_fast_batch > 50:
                            # 直接行人发车
                            self.car_slow_fast_batch = self.batchNum + random.randint(1, 50)
                            back_car_send_flag = True
                        else:
                            if batch_num == self.people_pass:
                                self.car_slow_fast_batch = self.batchNum + random.randint(1, 50)
                                back_car_send_flag = True
                        # 自然交通流发车
                        if back_car_send_flag:
                            # self.car_slow_fast_batch = self.batchNum + random.randint(1, 50)
                            dvp = Online.DynaVehiParam()
                            numbers = 1
                            dvp.vehiTypeCode = 1
                            dvp.laneNumber = random.randint(1, 2)
                            dvp.roadId = 586
                            dvp.dist = 0
                            dvp.speed = 15
                            dvp.color = f"#C0C0C0"
                            simu_iface.createGVehicle(dvp, True)
                elif scene_id == 'R3':
                    self.people_pass = 0
                    self.car_slow_fast_car = dict()
                    self.car_slow_fast_batch = 0
                    self.car_slow_dict = dict()
                    self.car_slow_batch = 0
                    self.car_over_target_car = dict()
                    self.target_confluence_batch = 0
                    # 交叉口直行
                    self.intersection_z3_right_car = dict()
                    self.intersection_z3_straight_car = dict()
                    # 交叉口左转
                    self.intersection_k2_right_car = dict()
                    self.intersection_k2_straight_car = dict()
                    self.back_traffic_K1_start_flow = dict()
                    self.bike = dict()
                    # 合流
                    vehicle_list = simu_iface.vehisInLink(611)
                    if len(vehicle_list) < 3:
                        if self.batchNum - self.confluence_batch > 30:
                            self.confluence_batch = self.batchNum + 20
                        elif self.batchNum == self.confluence_batch:
                            dvp = Online.DynaVehiParam()
                            dvp.vehiTypeCode = 1
                            dvp.laneNumber = 0
                            dvp.roadId = 611
                            dvp.dist = 0
                            dvp.speed = 15
                            dvp.color = car_color[self.target_in_area]
                            simu_iface.createGVehicle(dvp, True)
                elif scene_id == 'Z1':
                    # 行人穿行，自行车参数
                    self.people_pass = 0
                    # 走走停停场景参数
                    self.car_slow_fast_car = dict()
                    self.car_slow_fast_batch = 0
                    # 合流区场景参数
                    self.confluence_batch = 0
                    # 背景车超车场景
                    self.car_over_target_car = dict()
                    # 主车汇入
                    self.target_confluence_batch = 0
                    # 交叉口直行
                    self.intersection_z3_right_car = dict()
                    self.intersection_z3_straight_car = dict()
                    # 交叉口左转
                    self.intersection_k2_right_car = dict()
                    self.intersection_k2_straight_car = dict()
                    self.bike = dict()
                    self.back_traffic_K1_start_flow = dict()
                    vehicle_list = simu_iface.vehisInLink(603)
                    if len(vehicle_list) < 8:
                        # 减速并车
                        if self.batchNum - self.car_slow_batch > 22:
                            self.car_slow_batch = self.batchNum + 5
                        elif self.batchNum == self.car_slow_batch:
                            dvp = Online.DynaVehiParam()
                            numbers = [1, 2]
                            dvp.vehiTypeCode = 1
                            dvp.laneNumber = 0
                            dvp.roadId = 603
                            dvp.dist = 0
                            dvp.speed = 15
                            dvp.color = car_color[self.target_in_area]
                            vehicle = simu_iface.createGVehicle(dvp, True)
                            self.car_slow_dict[vehicle.id()] = vehicle.id()
                elif scene_id == 'Z2':
                    # 行人穿行，自行车参数
                    self.people_pass = 0
                    # 走走停停场景参数
                    self.car_slow_fast_car = dict()
                    self.car_slow_fast_batch = 0
                    # 合流区场景参数
                    self.confluence_batch = 0
                    # 减速场景参数
                    self.car_slow_dict = dict()
                    self.car_slow_batch = 0
                    # 主车汇入
                    self.target_confluence_batch = 0
                    # 交叉口直行
                    self.intersection_z3_right_car = dict()
                    self.intersection_z3_straight_car = dict()
                    # 交叉口左转
                    self.intersection_k2_right_car = dict()
                    self.intersection_k2_straight_car = dict()
                    self.bike = dict()
                    self.back_traffic_K1_start_flow = dict()
                    # 背景车超车
                    if len(self.car_over_target_car) < 1:
                        dvp = Online.DynaVehiParam()
                        dvp.vehiTypeCode = 1
                        dvp.laneNumber = 0
                        dvp.roadId = 181
                        dvp.dist = 0
                        dvp.speed = 30
                        dvp.color = car_color[self.target_in_area]
                        vehicle = simu_iface.createGVehicle(dvp, True)
                        self.car_over_target_car[vehicle.id()] = vehicle.id()
                elif scene_id == 'K2':
                    # 行人穿行，自行车参数
                    self.people_pass = 0
                    # 走走停停场景参数
                    self.car_slow_fast_car = dict()
                    self.car_slow_fast_batch_num = 0
                    # 0-正常 1-30km/h  2-50km/h
                    self.car_slow_fast_status = 0
                    self.car_slow_fast_batch = 0
                    # 合流区场景参数
                    self.confluence_batch = 0
                    # 减速场景参数
                    self.car_slow_dict = dict()
                    self.car_slow_batch = 0
                    # 背景车超车场景
                    self.car_over_target_car = dict()
                    # 主车汇入
                    self.target_confluence_batch = 0
                    # 交叉口左转
                    self.intersection_k2_right_car = dict()
                    self.intersection_k2_straight_car = dict()
                    self.bike = dict()
                    self.back_traffic_K1_start_flow = dict()
                    # 右转车
                    if len(self.intersection_z3_right_car) < 1:
                        dvp = Online.DynaVehiParam()
                        dvp.vehiTypeCode = 1
                        dvp.laneNumber = 0
                        dvp.roadId = 103
                        dvp.dist = 0
                        dvp.speed = 5
                        dvp.color = car_color[self.target_in_area]
                        vehicle = simu_iface.createGVehicle(dvp, True)
                        self.intersection_z3_right_car[vehicle.id()] = vehicle.id()
                    if len(self.intersection_z3_straight_car) < 1:
                        dvp = Online.DynaVehiParam()
                        dvp.vehiTypeCode = 1
                        dvp.laneNumber = 0
                        dvp.roadId = 228
                        dvp.dist = 0
                        dvp.speed = 5
                        dvp.color = car_color[self.target_in_area]
                        vehicle = simu_iface.createGVehicle(dvp, True)
                        self.intersection_z3_straight_car[vehicle.id()] = vehicle.id()
                elif scene_id == 'K1':
                    # 走走停停场景参数
                    self.car_slow_fast_car = dict()
                    self.car_slow_fast_batch_num = 0
                    # 0-正常 1-30km/h  2-50km/h
                    self.car_slow_fast_status = 0
                    self.car_slow_fast_batch = 0
                    # 合流区场景参数
                    self.confluence_batch = 0
                    # 减速场景参数
                    self.car_slow_dict = dict()
                    self.car_slow_batch = 0
                    # 背景车超车场景
                    self.car_over_target_car = dict()
                    # 交叉口直行
                    self.intersection_z3_right_car = dict()
                    self.intersection_z3_straight_car = dict()
                    # 交叉口左转
                    self.intersection_k2_right_car = dict()
                    self.intersection_k2_straight_car = dict()
                    self.bike = dict()
                    # print("K1开始发车。。。。。", self.batchNum, self.target_confluence_batch)
                    # 主车合流
                    if len(self.back_traffic_K1_start_flow) < 1:
                        dvp = Online.DynaVehiParam()
                        dvp.vehiTypeCode = 1
                        dvp.laneNumber = 0
                        dvp.roadId = 613
                        dvp.dist = 20
                        dvp.speed = 5
                        dvp.color = car_color[self.target_in_area]
                        vehicle1 = simu_iface.createGVehicle(dvp, True)
                        self.back_traffic_K1_start_flow[vehicle1.id()] = vehicle1.id()
                        dvp = Online.DynaVehiParam()
                        dvp.vehiTypeCode = 1
                        dvp.laneNumber = 0
                        dvp.roadId = 613
                        dvp.dist = 80
                        dvp.speed = 5
                        dvp.color = car_color[self.target_in_area]
                        vehicle1 = simu_iface.createGVehicle(dvp, True)
                        self.back_traffic_K1_start_flow[vehicle1.id()] = vehicle1.id()
                        dvp = Online.DynaVehiParam()
                        dvp.vehiTypeCode = 1
                        dvp.laneNumber = 0
                        dvp.roadId = 612
                        dvp.dist = 50
                        dvp.speed = 5
                        dvp.color = car_color[self.target_in_area]
                        vehicle1 = simu_iface.createGVehicle(dvp, True)
                        self.back_traffic_K1_start_flow[vehicle1.id()] = vehicle1.id()
                        dvp = Online.DynaVehiParam()
                        dvp.vehiTypeCode = 1
                        dvp.laneNumber = 1
                        dvp.roadId = 607
                        dvp.dist = 20
                        dvp.speed = 5
                        dvp.color = car_color[self.target_in_area]
                        vehicle1 = simu_iface.createGVehicle(dvp, True)
                        self.back_traffic_K1_start_flow[vehicle1.id()] = vehicle1.id()
                    if self.batchNum - self.target_confluence_batch > 30:
                        self.target_confluence_batch = self.batchNum + 15
                        # self.target_confluence_batch = self.batchNum + 15
                    elif self.batchNum == self.target_confluence_batch:
                        # print("K1开始发车。。。。。")
                        dvp = Online.DynaVehiParam()
                        dvp.vehiTypeCode = 1
                        dvp.laneNumber = 0
                        # 60%的概率选择613，40%的概率选择612
                        if self.i == 0:
                            dvp.roadId = 612
                        elif self.i % 4 == 0:
                            dvp.roadId = 612
                        else:
                            dvp.roadId = 613
                        # if self.target_confluence_batch % 4 == 0:
                        #     dvp.roadId = 612
                        # else:
                        #     dvp.roadId = 613
                        # road_numbers = [612, 613]
                        # dvp.roadId = random.choice(road_numbers)
                        dvp.dist = 0
                        dvp.speed = 5
                        dvp.color = car_color[self.target_in_area]
                        # if dvp.roadId == 612:
                        #     if random.random() < 0.3:
                        #         return
                        self.i = self.i + 1
                        vehicle1 = simu_iface.createGVehicle(dvp, True)
                        self.back_traffic_K1_start_flow[vehicle1.id()] = vehicle1.id()

                elif scene_id == 'Z3':
                    # 行人穿行，自行车参数
                    self.people_pass = 0
                    # 走走停停场景参数
                    self.car_slow_fast_car = dict()
                    self.car_slow_fast_batch_num = 0
                    # 0-正常 1-30km/h  2-50km/h
                    self.car_slow_fast_status = 0
                    self.car_slow_fast_batch = 0
                    # 合流区场景参数
                    self.confluence_batch = 0
                    # 减速场景参数
                    self.car_slow_dict = dict()
                    self.car_slow_batch = 0
                    # 背景车超车场景
                    self.car_over_target_car = dict()
                    # 主车汇入
                    self.target_confluence_batch = 0
                    # 交叉口直行
                    self.intersection_z3_right_car = dict()
                    self.intersection_z3_straight_car = dict()
                    self.back_traffic_K1_start_flow = dict()
                    # 交叉口左转
                    self.bike = dict()
                    # 右转车
                    if len(self.intersection_k2_right_car) < 1:
                        dvp = Online.DynaVehiParam()
                        dvp.vehiTypeCode = 1
                        dvp.laneNumber = 0
                        dvp.roadId = 103
                        dvp.dist = 0
                        dvp.speed = 5
                        dvp.color = car_color[self.target_in_area]
                        vehicle = simu_iface.createGVehicle(dvp, True)
                        self.intersection_k2_right_car[vehicle.id()] = vehicle.id()
                    # if len(self.intersection_straight_car_1) < 1:
                    #     dvp = Online.DynaVehiParam()
                    #     dvp.vehiTypeCode = 1
                    #     dvp.laneNumber = 0
                    #     dvp.roadId = 228
                    #     dvp.dist = 0
                    #     dvp.speed = 5
                    #     dvp.color = car_color[self.target_in_area]
                    #     vehicle = simu_iface.createGVehicle(dvp, True)
                    #     self.intersection_straight_car_1[vehicle.id()] = vehicle.id()
                    if len(self.intersection_k2_straight_car) < 1:
                        dvp = Online.DynaVehiParam()
                        dvp.vehiTypeCode = 1
                        dvp.laneNumber = 0
                        dvp.roadId = 615
                        dvp.dist = 0
                        dvp.speed = 5
                        dvp.color = car_color[self.target_in_area]
                        vehicle = simu_iface.createGVehicle(dvp, True)
                        self.intersection_k2_straight_car[vehicle.id()] = vehicle.id()
                elif scene_id == 'K3':
                    # 行人穿行，自行车参数
                    self.people_pass = 0
                    # 走走停停场景参数
                    self.car_slow_fast_car = dict()
                    self.car_slow_fast_batch_num = 0
                    # 0-正常 1-30km/h  2-50km/h
                    self.car_slow_fast_status = 0
                    self.car_slow_fast_batch = 0
                    # 合流区场景参数
                    self.confluence_batch = 0
                    # 减速场景参数
                    self.car_slow_dict = dict()
                    self.car_slow_batch = 0
                    # 背景车超车场景
                    self.car_over_target_car = dict()
                    # 主车汇入
                    self.target_confluence_batch = 0
                    # 交叉口直行
                    self.intersection_z3_right_car = dict()
                    self.intersection_z3_straight_car = dict()
                    # 交叉口左转
                    self.intersection_k2_right_car = dict()
                    self.intersection_k2_straight_car = dict()
                    self.back_traffic_K1_start_flow = dict()
                    if len(self.bike) < 1:
                        dvp = Online.DynaVehiParam()
                        dvp.vehiTypeCode = 6
                        dvp.laneNumber = 0
                        dvp.roadId = 586
                        dvp.dist = 0
                        dvp.speed = 2
                        dvp.color = car_color[self.target_in_area]
                        vehicle = simu_iface.createGVehicle(dvp, True)
                        self.bike[vehicle.id()] = vehicle.id()

        # 模拟器真车处理逻辑
        target_all_car_dict = self.target_all_car_dict.copy()
        target_start_car_dict = self.target_start_car_dict.copy()
        for carId in target_all_car_dict.keys():
            car = target_all_car_dict[carId]
            longitude = car['longitude']
            latitude = car['latitude']
            # print('real_car:', longitude, latitude)
            self.target_lon = longitude
            self.target_lat = latitude
            lon, lat = self.p(longitude, latitude, inverse=False)
            if find_key_by_value(target_start_car_dict, carId) is not None:
                # 首先判断该车辆是否在发车列表中
                if carId in target_start_car_dict.values():
                    car_id = find_key_by_value(target_start_car_dict, carId)
                    # 认定该车辆已经存在于发车列表中，此时更新该车辆的位置
                    vehicle = simu_iface.getVehicle(car_id)
                    if vehicle is not None:
                        # 获取驾驶行为
                        vehicle_driving = vehicle.vehicleDriving()
                        q_point = QPointF()
                        q_point.setX(lon)
                        q_point.setY(-lat)
                        # 获取离目标点最近的坐标，共9个单元格，一般 lLocation是最近的
                        l_location = net_iface.locateOnCrid(q_point, 9)
                        if len(l_location) > 0:
                            vehicle_driving.move(l_location[0].pLaneObject, l_location[0].distToStart)
                        else:
                            print('未找到该车辆的位置信息')
                        continue
            else:
                # 进行发车操作
                q_point = QPointF()
                q_point.setX(lon)
                q_point.setY(-lat)
                # 获取离目标点最近的坐标，共9个单元格，一般 lLocation是最近的
                l_location = net_iface.locateOnCrid(q_point, 9)
                # 判断获取到最近的坐标集合是否为空
                if len(l_location) > 0:
                    p_lane_object = l_location[0].pLaneObject
                    # 路段发车
                    if isinstance(p_lane_object, ILane):
                        l_link = p_lane_object.link()
                        ll_id = l_link.id()
                        sec_from_connect_link = l_link.fromConnectors()
                        if len(sec_from_connect_link) > 0:
                            iConnector = sec_from_connect_link[0]
                            lLink = iConnector.fromLink()
                            ll_id = lLink.id()
                            print('上游道路id', lLink.id())
                        # 发车信息初始化
                        dvp = Online.DynaVehiParam()
                        dvp.vehiTypeCode = car_real_type[car['vehicleType']]
                        # 根据车辆类型在指定车道发车（机动车仅能在机动车道行驶，非机动车(5,6,7)则能在所有车道行驶） 目前能够精确到具体车道
                        dvp.laneNumber = p_lane_object.number()
                        # 这里理论上需要计算发车点距离路段起点的位置
                        # （会首先获取当前车道所有的起点位置，计算起点位置距离车辆经纬度的距离，作为发车点距离路段起点的距离）
                        dvp.roadId = l_link.id()
                        dvp.dist = l_location[0].distToStart
                        dvp.speed = 0
                        dvp.color = f"#4169E1"
                        print('主车所在道路ID:', dvp.roadId, '所在车道', dvp.laneNumber)
                        for vehicle in all_vehicle_start:
                            v_road = vehicle.roadId()
                            v_lane = vehicle.lane()
                            if dvp.roadId == v_road and dvp.laneNumber == v_lane.number():
                                print('主车所在道路ID:', dvp.roadId, '所在车道', dvp.laneNumber,
                                      '移除所在道路ID:', v_road, '所在车道', v_lane.number())
                                simu_iface.stopVehicleDriving(vehicle)
                            if dvp.roadId == ll_id and dvp.laneNumber == v_lane.number():
                                print('主车所在道路ID:', dvp.roadId, '所在车道', dvp.laneNumber,
                                      '移除所在道路ID:', v_road, '所在车道', v_lane.number())
                                simu_iface.stopVehicleDriving(vehicle)

                        vehicle = simu_iface.createGVehicle(dvp, True)
                        if vehicle is not None:
                            self.target_start_car_dict[vehicle.id()] = carId
                        else:
                            # pass
                            print('路段车辆创建失败')

                    # 连接段发车
                    else:
                        # 发车信息初始化
                        dvp = Online.DynaVehiParam()
                        dvp.vehiTypeCode = car_real_type[car['vehicleType']]
                        # 获取连接段
                        connector = p_lane_object.connector()
                        # 根据车辆类型在指定车道发车（机动车仅能在机动车道行驶，非机动车(5,6,7)则能在所有车道行驶） 目前能够精确到具体车道
                        from_link = connector.fromLink()
                        dvp.laneNumber = random.randint(1, 2)
                        # 这里理论上需要计算发车点距离路段起点的位置
                        # （会首先获取当前车道所有的起点位置，计算起点位置距离车辆经纬度的距离，作为发车点距离路段起点的距离）
                        dvp.roadId = from_link.id()
                        dvp.dist = from_link.length() - 1
                        dvp.speed = 0
                        dvp.color = f"#4169E1"
                        vehicle = simu_iface.createGVehicle(dvp, True)
                        if vehicle is not None:
                            self.target_start_car_dict[vehicle.id()] = carId
                        else:
                            pass


def find_key_by_value(dictionary, value):
    for key, val in dictionary.items():
        if val == value:
            return key
    return None

#
# class MyServer:
#     async def websocket_handler(self, websocket, path):
#         global send_data
#         while True:
#             # async for message in websocket:
#             await websocket.send(send_data)
#             time.sleep(0.1)
#
#     def start_websocket_server(self):
#         loop = asyncio.new_event_loop()  # 在新线程中创建新的事件循环
#         asyncio.set_event_loop(loop)
#         start_server = websockets.serve(self.websocket_handler, "10.100.12.46", 8765)
#         loop.run_until_complete(start_server)
#         loop.run_forever()
